
//Declaración de Constantes de Motores
int motor1_a = 3;
int motor1_b = 5;
int motor2_a = 6;
int motor2_b = 9;
int motor3_a = 10;
int motor3_b = 11;

byte inByte[4];
byte dir;
byte num_motor;


// Setup
void setup() {                
  // initialize the digital pin as an output.
  pinMode(motor1_a, OUTPUT);
  pinMode(motor1_b, OUTPUT);  
  pinMode(motor2_a, OUTPUT);  
  pinMode(motor2_b, OUTPUT);  
  pinMode(motor3_a, OUTPUT);  
  pinMode(motor3_b, OUTPUT);
  
  // set them to LOW for security
  digitalWrite(motor1_a, LOW);
  digitalWrite(motor1_b, LOW);
  digitalWrite(motor2_a, LOW);
  digitalWrite(motor2_b, LOW);
  digitalWrite(motor3_a, LOW);
  digitalWrite(motor3_b, LOW);
  
  //Inicia Conexión Serial
  Serial.begin(9600);
}

// Loop
void loop() {
  if (Serial.available() > 0) {
                
    // read the incoming byte:
    inByte[0] = Serial.read();
    
    // verifica si es el byte de inicio
    if(inByte[0] == 0x80){
      while(!(Serial.available() > 0));
      inByte[1] = Serial.read();
      while(!(Serial.available() > 0));
      inByte[2] = Serial.read();
      while(!(Serial.available() > 0));
      inByte[3] = Serial.read();
      
      // verifica si es el byte de fin
      if(inByte[3] == 0xF0){
        dir = inByte[1] & 0x1;
        num_motor = inByte[1]>>1;
        
        switch(num_motor){
          case 1:
            if(dir == 0){
              digitalWrite(motor1_a, LOW);
              analogWrite(motor1_b, inByte[2]);
            }else {
              digitalWrite(motor1_b, LOW);
              analogWrite(motor1_a, inByte[2]);
            }
            break;
            
          case 2:
            if(dir == 0){
              digitalWrite(motor2_a, LOW);
              analogWrite(motor2_b, inByte[2]);
            }else {
              digitalWrite(motor2_b, LOW);
              analogWrite(motor2_a, inByte[2]);
            }
            break;
            
          case 3:
            if(dir == 0){
              digitalWrite(motor3_a, LOW);
              analogWrite(motor3_b, inByte[2]);
            }else {
              digitalWrite(motor3_b, LOW);
              analogWrite(motor3_a, inByte[2]);
            }
            break;
        }
        
      }
    }

  }
}
